// mdvctrl - A video-driven controller for MD-25 motor drivers
// Copyright (C) 2010  Nuno J. Silva
//
// This program is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation, version 3 of the License.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program.  If not, see <http://www.gnu.org/licenses/>.

#include <cv.h>
#include <highgui.h>
#include "fatal.h"


// Tells which capture device should be used. -1 means any, no matter
// which one. If you want a specific one, change this to the right id
// (>= 0).
#define CAMERA_ID -1


// Initialize OpenCV stuff: video capture device (a file or a "real"
// video device), and optionally also a video record device.
//
// Takes two pointers to pointers as arguments, which will store the
// address of the new OpenCV capture and record devices, and also the
// names of the source and destination video files.
//
// The resulting capture device captures from a camera iff the source
// filename is NULL; 
//
// It only initializes the video writer iff the destination filename
// is not NULL.
//
void opencv_init(const char * source_filename, const char * dest_filename, CvCapture ** capture, CvVideoWriter ** writer) {
  // This seems to be needed to use OpenCV windows.
  cvStartWindowThread();

  // Create the windows that will be used to display frames.
  cvNamedWindow("Window");
  cvNamedWindow("Gray");


  if (capture == NULL) {
    fatal ("no capture device to initialize");
  }

  if (source_filename == NULL) {
    *capture = cvCaptureFromCAM(CAMERA_ID);
  } else {
    *capture = cvCaptureFromFile(source_filename);
  }
  
  // This can happen, e.g. when OpenCV wasn't built with support for
  // the chosen kind of input.
  if ((*capture) == NULL) {
    fatal ("failure while initializing input stream: no stream returned");
  }

  if (dest_filename != NULL) {
    if (writer == NULL) {
      fatal ("no recording device to initialize");
    }

    int framerate = 100;

    *writer = NULL;
    *writer = cvCreateVideoWriter (dest_filename, 
				   CV_FOURCC ('P','I','M','1'),
				   framerate, cvSize (160,120), 1);

    if ((*writer) == NULL) {
      fatal ("failure while initializing output stream: no stream returned");
    }
  }
}

// Grab a frame and make the frame smaller and black-and-white.
IplImage* get_cooked_frame (CvCapture * capture, CvVideoWriter * writer) {
  IplImage* bigframe = cvQueryFrame(capture);
  if (bigframe == NULL) {
    // If for some reason there's no frame yet, skip this iteration
    // of the loop.
    return NULL;
  }

  // Conversion chain. We get arbitrary size color frame, we want
  // black-and-white ("binary", not grayscale) 160x120. To make it
  // even funnier, the binary frame is still a grayscale-depth
  // frame.
  IplImage* frame = cvCreateImage(cvSize(160,120), bigframe->depth, bigframe->nChannels);
  cvResize (bigframe,frame,CV_INTER_LINEAR);

  if (writer != NULL) {
    cvWriteFrame(writer,frame);
  }

  IplImage* gray_image = cvCreateImage(cvGetSize(frame), IPL_DEPTH_8U, 1);
  cvCvtColor(frame,gray_image,CV_RGB2GRAY);
  IplImage* bw_image = cvCreateImage(cvGetSize(frame), IPL_DEPTH_8U, 1);

  // As it is possible, apply the threshold *and* negate at the
  // same time.
  cvShowImage("Gray", gray_image);

  cvThreshold(gray_image, bw_image, 100, 255, CV_THRESH_BINARY_INV);
      
  // We wanna some pics!
  cvShowImage("Window", bw_image);

  return bw_image;
}
